COMMENT ⊗   VALID 00004 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	EULER  - PROGRAMS FOR CONVERTING TRANSFORMS ↔ EULER ANGLES
C00004 00003	"EULER" COMPUTES THE EULER ANGLES DESCRIBING AN ARM POSITION GIVEN
C00007 00004	"UNEUL" COMPUTES A TRANSFORM MATRIX REPRESENTING HAND POSITION AND 
C00011 ENDMK
C⊗;
;EULER  - PROGRAMS FOR CONVERTING TRANSFORMS ↔ EULER ANGLES

.TITLE EULER

COMMENT ⊗
	added by MSM 11/10/78

The relationship between the Euler angles outlined here
and the trans from which it is derived seems to be as follows:

	T =  TRANS(ROT(ZHAT,E[4]-90)*ROT(YHAT,E[5]+90)*ROT(ZHAT,E[6]),
			VECTOR(E[1],E[2],E[3]))

	in POINTY printouts  E[1]...E[6] correspond to X Y Z O A T

It appears also that O A T all lie in the range -90 ≤ O A T ≤ 90

	If we prefer to think in terms of

	T  = TRANS(ROT(ZHAT,THETA)*ROT(YHAT,PHI)*ROT(ZHAT,W),VECTOR(X,Y,Z))

the corresponding values are

	THETA = O - 90;		-90≤O≤90	180≤THETA≤0
	PHI   = A + 90;		-90≤A≤90	0≤PHI≤180
	W     = T;		-90≤W≤90	-90≤T≤90

⊗ ;

;DEFINITIONS

T21==4
T31==10
T12==14
T22==20
T32==24
T13==30
T23==34
T33==40
T14==44
T24==50
T34==54

.IFNZ STDALN
	.MACRO CODE
	.ENDM
	.MACRO DATA
	.ENDM
.ENDC
;CONSTANTS
DATA
CC01:	.FLT2	 0.0100 
TODEG:	.FLT2	 57.2958
CODE
;"EULER" COMPUTES THE EULER ANGLES DESCRIBING AN ARM POSITION GIVEN
;A TRANSFORM IN TABLE COORDINATES.  A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#T,R0		;LOAD ADDR. OF TRANSFORM "T"
;		MOV	#E,R1		;LOAD ADDR. OF ARRAY TO 
;					;  RETURN THE EULER ANGLES
;		JSR	PC,EULER	;CALLED USING THE PC
;
;THE TRANSFORM "T" MUST BE STORED BY COLUMNS AND THE ARRAY "E" MUST
;BE AT LEAST 6 ELEMENTS LONG.  ALL NUMBERS SHOULD BE IN SINGLE
;PRECISION FLOATING POINT.

;EXECUTION TIME:

;REGISTERS USED:
;
;	R0, R1 PASS ARGUMENTS AND R1 IS ALTERED BY "EULER"
;	AC0,AC1,AC2,AC3,AC4 ARE GARBAGED

;START OF "EULER"

EULER:	MOV	T14(R0),(R1)+	;COPY X LOCATION INTO E(1)
	MOV	T14+2(R0),(R1)+
	MOV	T24(R0),(R1)+	;COPY Y LOCATION INTO E(2)
	MOV	T24+2(R0),(R1)+
	MOV	T34(R0),(R1)+	;COPY Z LOCATION INTO E(3)
	MOV	T34+2(R0),(R1)+
	LDF	T13(R0),AC0	;COMPUTE CO2←SQRT(T[1,3]↑2+T[2,3]↑2)
	MULF	AC0,AC0
	LDF	T23(R0),AC1
	MULF	AC1,AC1
	ADDF	AC1,AC0
	JSR	PC,SQRTF
	STF	AC0,AC4		;SAVE CO2 FOR LATER
	STF	AC0,AC1		;COMPUTE ATAN(-T[3,3]/CO2)
	LDF	T33(R0),AC0
	NEGF	AC0
	JSR	PC,ATAN2
	STF	AC0,4(R1)	;THIS IS E(5)
	LDF	AC4,AC0		;CHECK IF CO2 ALMOST ZERO
	CMPF	CC01,AC0
	CFCC
	BLE	CO2BIG		;IF CO2<0.01
	LDF	T22(R0),AC0	;THEN E(4)←ATAN(T[2,2]/T[1,2])
	LDF	T12(R0),AC1
	JSR	PC,ATAN2
	STF	AC0,(R1)
	CLR	10(R1)		;AND E(6)←0
	CLR	12(R1)
	RTS	PC		;RETURN
CO2BIG:	LDF	T13(R0),AC0	;ELSE E[4]←ATAN(T[1,3]/-T[2,3])
	LDF	T23(R0),AC1
	NEGF	AC1
	JSR	PC,ATAN2
	STF	AC0,(R1)
	LDF	T32(R0),AC0	;AND E[6]←ATAN(T[3,2]/-[3,1])
	LDF	T31(R0),AC1
	NEGF	AC1
	JSR	PC,ATAN2
	STF	AC0,10(R1)
	RTS	PC

PUTLOC	LEULER,EULER

;END OF "EULER"
;"UNEUL" COMPUTES A TRANSFORM MATRIX REPRESENTING HAND POSITION AND 
;ORIENTATION IN TABLE COORDINATES GIVEN THE EULER ANGLES OF THE 
;LOCATION.  A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#T,R0		;LOAD ADDR. OF TRANSFORM "T"
;		MOV	#E,R1		;LOAD ADDR. OF EULER ANGLE ARRAY
;		JSR	PC,UNEUL	;CALLED USING THE PC
;
;THE TRANSFORM "T" WILL BE STORED BY COLUMNS AND THE ARRAY "E" MUST
;CONTAIN THE 6 EULER ANGLES.  ALL NUMBERS SHOULD BE IN SINGLE
;PRECISION FLOATING POINT.

;EXECUTION TIME:

;REGISTERS USED:
;
;	R0, R1 PASS ARGUMENTS AND R1 IS ALTERED BY "UNEUL"
;	AC0,AC1,AC2,AC3,AC4 ARE GARBAGED

;START OF "UNEUL"

UNEUL:	MOV	(R1)+,T14(R0)	;COPY X LOCATION INTO T[1,4]
	MOV	(R1)+,T14+2(R0)
	MOV	(R1)+,T24(R0)	;COPY Y LOCATION INTO T[2,4]
	MOV	(R1)+,T24+2(R0)
	MOV	(R1)+,T34(R0)	;COPY Z LOCATION INTO T[3,4]
	MOV	(R1)+,T34+2(R0)
	LDF	(R1)+,AC0	;COMPUTE THE SINE/COSINE OF E[4]
	JSR	PC,SNCOS
	STF	AC0,SI1
	STF	AC1,CO1
	LDF	(R1)+,AC0	;COMPUTE THE SINE/COSINE OF E[5]
	JSR	PC,SNCOS
	STF	AC0,SI2
	STF	AC1,CO2
	LDF	(R1),AC0	;COMPUTE THE SINE/COSINE OF E[6]
	JSR	PC,SNCOS
	LDF	SI1,AC2		;T[1,1]←-SI1*SI2*CO3+CO1*SI3
	MULF	SI2,AC2
	STF	AC2,AC4		;SAVE SI1*SI2
	MULF	AC1,AC2
	LDF	CO1,AC3
	MULF	AC0,AC3
	SUBF	AC2,AC3
	STF	AC3,(R0)
	LDF	AC4,AC2		;T[1,2]←SI1*SI2*SI3+CO1*CO3
	MULF	AC0,AC2
	LDF	CO1,AC3
	MULF	AC1,AC3
	ADDF	AC3,AC2
	STF	AC2,T12(R0)
	MULF	SI2,AC3		;T[2,1]←CO1*CO3*SI2+SI1*SI3
	LDF	SI1,AC2
	MULF	AC0,AC2
	ADDF	AC3,AC2
	STF	AC2,T21(R0)
	LDF	CO1,AC2		;T[2,2]←-CO1*SI2*SI3+SI1*CO3
	MULF	SI2,AC2
	MULF	AC0,AC2
	LDF	SI1,AC3
	MULF	AC1,AC3
	SUBF	AC2,AC3
	STF	AC3,T22(R0)
	LDF	CO2,AC2		;T[1,3]←SI1*CO2
	LDF	SI1,AC3
	MULF	AC2,AC3
	STF	AC3,T13(R0)
	MULF	AC2,AC0		;T[3,2]←CO2*SI3
	STF	AC0,T32(R0)
	NEGF	AC2
	LDF	CO1,AC3		;T[2,3]←-CO1*CO2
	MULF	AC2,AC3
	STF	AC3,T23(R0)
	MULF	AC2,AC1		;T[3,1]←-CO2*CO3
	STF	AC1,T31(R0)
	LDF	SI2,AC0		;T[3,3]←-SI2
	NEGF	AC0
	STF	AC0,T33(R0)
	RTS	PC		;RETURN
DATA
SI1:	.WORD	0,0
CO1:	.WORD	0,0
SI2:	.WORD	0,0
CO2:	.WORD	0,0
CODE
;END OF "UNEUL"